#include "StdAfx.h"
#include "MeshGrid.h"
#include "OffGridLinks.h"

//#pragma optimize("", off)
//#pragma inline_depth(0)


namespace MNM
{

	const real_t MeshGrid::kMinPullingThreshold = real_t(0.05f);
	const real_t MeshGrid::kMaxPullingThreshold = real_t(0.95f); 

	enum { SideCount = 14, };
	
	static const int NeighbourOffset_MeshGrid[14][3] =
	{
		{ 1, 0, 0},
		{ 1, 0, 1},
		{ 1, 0,-1},

		{ 0, 1, 0},
		{ 0, 1, 1},
		{ 0, 1,-1},

		{ 0, 0, 1},

		{-1, 0, 0},
		{-1, 0,-1},
		{-1, 0, 1},

		{ 0,-1, 0},
		{ 0,-1,-1},
		{ 0,-1, 1},

		{ 0, 0,-1},
	};

	MeshGrid::MeshGrid()
		: m_tiles(0)
		, m_tileCount(0)
		, m_tileCapacity(0)
		, m_triangleCount(0)
	{
		m_nextLinkedTriangles.reserve(32);
	}

	MeshGrid::~MeshGrid()
	{
		for (size_t i = 0; i < m_tileCapacity; ++i)
			m_tiles[i].tile.Destroy();

		SAFE_DELETE_ARRAY(m_tiles);
	}

	void MeshGrid::Init(const Params& params)
	{
		assert(m_tiles == NULL);
		assert(m_tileCapacity == 0);

		m_params = params;

		Grow(params.tileCount);
	}

	size_t MeshGrid::GetTriangles(aabb_t aabb, TriangleID* triangles, size_t maxTriCount) const
	{
		const size_t minX = (std::max(aabb.min.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint();
		const size_t minY = (std::max(aabb.min.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint();
		const size_t minZ = (std::max(aabb.min.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint();

		const size_t maxX = (std::max(aabb.max.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint();
		const size_t maxY = (std::max(aabb.max.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint();
		const size_t maxZ = (std::max(aabb.max.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint();

		size_t triCount = 0;

		for (uint y = minY; y <= maxY; ++y)
		{
			for (uint x = minX; x <= maxX; ++x)
			{
				for (uint z = minZ; z <= maxZ; ++z)
				{
					if (const TileID tileID = GetTileID(x, y, z))
					{
						const Tile& tile = GetTile(tileID);

						const vector3_t tileOrigin(
							real_t(x * m_params.tileSize.x),
							real_t(y * m_params.tileSize.y),
							real_t(z * m_params.tileSize.z));

						aabb_t relative(aabb);
						relative.min = vector3_t::maximize(relative.min - tileOrigin, vector3_t(0, 0, 0));
						relative.max = vector3_t::minimize(relative.max - tileOrigin, 
							vector3_t(m_params.tileSize.x, m_params.tileSize.y, m_params.tileSize.z));

						if (!tile.nodeCount)
						{
							for (size_t i = 0; i < tile.triangleCount; ++i)
							{
								const Tile::Triangle& triangle = tile.triangles[i];
								
								const Tile::Vertex& v0 = tile.vertices[triangle.vertex[0]];
								const Tile::Vertex& v1 = tile.vertices[triangle.vertex[1]];
								const Tile::Vertex& v2 = tile.vertices[triangle.vertex[2]];

								const aabb_t triaabb(vector3_t::minimize(v0, v1, v2), vector3_t::maximize(v0, v1, v2));

								if (relative.overlaps(triaabb))
								{
									triangles[triCount++] = ComputeTriangleID(tileID, i);

									if (triCount == maxTriCount)
										return triCount;
								}
							}
						}
						else
						{
							size_t nodeID = 0;
							const size_t nodeCount = tile.nodeCount;

							while (nodeID < nodeCount)
							{
								const Tile::BVNode& node = tile.nodes[nodeID];

								if (!relative.overlaps(node.aabb))
									nodeID += node.leaf ? 1 : node.offset;
								else
								{
									++nodeID;

									if (node.leaf)
									{
										triangles[triCount++] = ComputeTriangleID(tileID, node.offset);

										if (triCount == maxTriCount)
											return triCount;
									}
								}
							}
						}
					}
				}
			}
		}

		return triCount;
	}

	TriangleID MeshGrid::GetTriangleAt(const vector3_t& location, const real_t verticalDownwardRange, const real_t verticalUpwardRange) const
	{

		const MNM::aabb_t aabb(
			MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z - verticalDownwardRange)),
			MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z + verticalUpwardRange)));

		const size_t MaxTriCandidateCount = 1024;
		TriangleID candidates[MaxTriCandidateCount];

		TriangleID closestID = 0;

		const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount);
		MNM::real_t distMinSq = MNM::real_t::max();

		if (candidateCount)
		{
			MNM::vector3_t start = location;
			MNM::vector3_t a, b, c;

			for (size_t i = 0; i < candidateCount; ++i)
			{
				GetVertices(candidates[i], a, b, c);

				if (PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c)))
				{
					const MNM::vector3_t ptClosest = ClosestPtPointTriangle(location, a, b, c);
					const MNM::real_t dSq = (ptClosest - location).lenSq();

					if (dSq < distMinSq)
					{
						distMinSq = dSq;
						closestID = candidates[i];
					}
				}
			}
		}

		return closestID;
	}

	bool MeshGrid::IsTriangleAcceptableForLocation(const vector3_t& location, TriangleID triangleID) const
	{
		const MNM::real_t range = MNM::real_t(1.0f);
		bool found = false;
		if (triangleID)
		{
			const MNM::aabb_t aabb(
				MNM::vector3_t(MNM::real_t(location.x - range), MNM::real_t(location.y - range), MNM::real_t(location.z - range)),
				MNM::vector3_t(MNM::real_t(location.x + range), MNM::real_t(location.y + range), MNM::real_t(location.z + range)));

			const size_t MaxTriCandidateCount = 1024;
			TriangleID candidates[MaxTriCandidateCount];

			const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount);
			MNM::real_t distMinSq = MNM::real_t::max();

			if (candidateCount)
			{
				MNM::vector3_t start = location;
				MNM::vector3_t a, b, c;

				for (size_t i = 0; i < candidateCount; ++i)
				{
					GetVertices(candidates[i], a, b, c);

					if (candidates[i] == triangleID && PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c)))
					{
						return true;
					}
				}
			}
		}
		
		return false;
	}

	TriangleID MeshGrid::GetClosestTriangle(const vector3_t& location, real_t vrange, real_t hrange, real_t* distSq,
		vector3_t* closest) const
	{
		const MNM::aabb_t aabb(
			MNM::vector3_t(MNM::real_t(location.x - hrange), MNM::real_t(location.y - hrange), MNM::real_t(location.z - vrange)),
			MNM::vector3_t(MNM::real_t(location.x + hrange), MNM::real_t(location.y + hrange), MNM::real_t(location.z + vrange)));

		const size_t MaxTriCandidateCount = 1024;
		TriangleID candidates[MaxTriCandidateCount];

		TriangleID closestID = 0;

		const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount);
		MNM::real_t distMinSq = MNM::real_t::max();

		if (candidateCount)
		{
			MNM::vector3_t start = location;
			MNM::vector3_t a, b, c;

			for (size_t i = 0; i < candidateCount; ++i)
			{
				GetVertices(candidates[i], a, b, c);

				const MNM::vector3_t ptClosest = ClosestPtPointTriangle(location, a, b, c);
				const MNM::real_t dSq = (ptClosest - location).lenSq();
				
				if (dSq < distMinSq)
				{
					if (closest)
						*closest = ptClosest;

					distMinSq = dSq;
					closestID = candidates[i];
				}
			}
		}

		if (distSq)
			*distSq = distMinSq;

		return closestID;
	}

	bool MeshGrid::GetVertices(TriangleID triangleID, vector3_t& v0, vector3_t& v1, vector3_t& v2) const
	{
		if (const TileID tileID = ComputeTileID(triangleID))
		{
			const TileContainer& container = m_tiles[tileID - 1];

			const vector3_t origin(
				real_t(container.x * m_params.tileSize.x),
				real_t(container.y * m_params.tileSize.y),
				real_t(container.z * m_params.tileSize.z));

			const Tile::Triangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
			v0 = origin + vector3_t(container.tile.vertices[triangle.vertex[0]]);
			v1 = origin + vector3_t(container.tile.vertices[triangle.vertex[1]]);
			v2 = origin + vector3_t(container.tile.vertices[triangle.vertex[2]]);
		}

		return false;
	}

	bool MeshGrid::GetTriangle(TriangleID triangleID, Tile::Triangle& triangle) const
	{
		if (const TileID tileID = ComputeTileID(triangleID))
		{
			const TileContainer& container = m_tiles[tileID - 1];
			triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
		
			return true;
		}

		return false;
	}

	bool MeshGrid::PushPointInsideTriangle(const TriangleID triangleID, vector3_t& location, real_t amount) const
	{
		if(amount <= 0)
			return false;

		vector3_t v0, v1, v2;
		if (const TileID tileID = ComputeTileID(triangleID))
		{
			const TileContainer& container = m_tiles[tileID - 1];
			const vector3_t origin(
				real_t(container.x * m_params.tileSize.x),
				real_t(container.y * m_params.tileSize.y),
				real_t(container.z * m_params.tileSize.z));
			const vector3_t locationTileOffsetted = (location - origin);

			const Tile::Triangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
			v0 = vector3_t(container.tile.vertices[triangle.vertex[0]]);
			v1 = vector3_t(container.tile.vertices[triangle.vertex[1]]);
			v2 = vector3_t(container.tile.vertices[triangle.vertex[2]]);

			const vector3_t triangleCenter = ((v0 + v1 + v2) / MNM::real_t(3.0f));
			const vector3_t locationToCenter = triangleCenter - locationTileOffsetted;
			const real_t locationToCenterLen = locationToCenter.len();

			if(locationToCenterLen > amount)
			{
				location += (locationToCenter / locationToCenterLen) * amount;
			}
			else
			{
				// If locationToCenterLen is smaller than the amount I wanna push
				// the point, then it's safer use directly the center position
				// otherwise the point could end up outside the other side of the triangle
				location = triangleCenter + origin;
			}
			return true;
		}

		return false;
	}
	
	MeshGrid::WayQueryResult MeshGrid::FindWay( MeshGrid::WayQueryRequest& inputRequest, WayTriangleData* outputWay, size_t maxWayTriCount) const
	{
		if (maxWayTriCount < 2)
		{
			return WayQueryResult(0, true);
		}

		if (inputRequest.From() && inputRequest.To())
		{
			if (inputRequest.From() == inputRequest.To())
			{
				outputWay[0] = WayTriangleData(inputRequest.From(), 0);
				outputWay[1] = WayTriangleData(inputRequest.To(), 0);
				
				return WayQueryResult(2, true);
			}
			else
			{
				const real_t heuristicFactor1(0.5f * (1.0f + (1.0f /(4.0f * sqrtf(3.0f)))));
				const real_t heuristicFactor2(1.0f / sqrtf(3.0f));

				vector3_t v0, v1, v2;
				GetVertices(inputRequest.To(), v0, v1, v2);
				const vector3_t endLocation = (v0 + v1 + v2) * real_t::fraction(1, 3);

				WayTriangleData lastBestNodeID(inputRequest.From(), 0);

				AStarOpenList& openList = inputRequest.GetOpenList();
				while (openList.CanDoStep())
				{
					// switch the smallest element with the last one and pop the last element
					AStarOpenList::OpenNodeListElement element = openList.PopBestNode();
					WayTriangleData bestNodeID = element.triData;

					lastBestNodeID = bestNodeID;
					IF_UNLIKELY (bestNodeID.triangleID == inputRequest.To())
					{
						openList.StepDone();
						break;
					}

					AStarOpenList::Node* bestNode = element.pNode;

					const TileID tileID = ComputeTileID(bestNodeID.triangleID);

					// Clear from previous step
					m_nextLinkedTriangles.clear();

					if (tileID)
					{
						const TileContainer& container = m_tiles[tileID - 1];
						const Tile& tile = container.tile;
						const uint16 triangleIdx = ComputeTriangleIndex(bestNodeID.triangleID);
						const Tile::Triangle& triangle = tile.triangles[triangleIdx];

						//Gather all accessible triangles first

						for (size_t l = 0; l < triangle.linkCount; ++l)
						{
							const Tile::Link& link = tile.links[triangle.firstLink + l];

							WayTriangleData nextTri(0, 0);

							if (link.side == Tile::Link::Internal)
							{
								nextTri.triangleID = ComputeTriangleID(tileID, link.triangle);
							}
							else if (link.side == Tile::Link::OffMesh)
							{
								OffMeshNavigation::QueryLinksResult links = inputRequest.GetOffMeshNavigation().GetLinksForTriangle(bestNodeID.triangleID, link.triangle);
								while(nextTri = links.GetNextTriangle())
								{
									if (inputRequest.CanUseOffMeshLink(nextTri.offMeshLinkID))
									{
										m_nextLinkedTriangles.push_back(nextTri);
									}
								}
								continue;
							}
							else
							{
								TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side);
								nextTri.triangleID = ComputeTriangleID(neighbourTileID, link.triangle);
							}

							m_nextLinkedTriangles.push_back(nextTri);

							//////////////////////////////////////////////////////////////////////////
							// NOTE: This is user defined only at compile time

							BreakOnInvalidTriangle(nextTri.triangleID, m_tileCount);

							//////////////////////////////////////////////////////////////////////////
						}
					}
					else
					{
						AIError("MeshGrid::FindWay - Bad Navmesh data Tile: %d, Triangle: %d, skipping ", tileID, ComputeTriangleIndex(bestNodeID.triangleID));
						BreakOnInvalidTriangle(bestNodeID.triangleID, m_tileCount);
					}
					

					const size_t triangleCount = m_nextLinkedTriangles.size();
					for (size_t t = 0; t < triangleCount; ++t)
					{
						WayTriangleData nextTri = m_nextLinkedTriangles[t];
						
						if (nextTri == bestNode->prevTriangle)
							continue;
						
						AStarOpenList::Node* nextNode = NULL;
						const bool inserted = openList.InsertNode(nextTri, &nextNode);

						assert(nextNode);

						if (inserted)
						{
							vector3_t vertice0, vertice1, vertice2;
							GetVertices(nextTri.triangleID, vertice0, vertice1, vertice2);

							nextNode->location = (vertice0 + vertice1 + vertice2) * real_t::fraction(1, 3);
							nextNode->open = false;
						}
						
						//Euclidean distance
						//const real_t heuristic = (endLocation - nextNode->location).len() * real_t::fraction(1015, 1000);
						//const real_t stepCost = (bestNode->location - nextNode->location).len();

						//Euclidean distance approximation
						const vector3_t targetDistance = endLocation - nextNode->location;
						const real_t targetDistAbsX = fabsf(targetDistance.x), targetDistAbsY = fabsf(targetDistance.y), targetDistAbsZ = fabsf(targetDistance.z); 
						const real_t heuristic = heuristicFactor1 * min( heuristicFactor2 * (targetDistAbsX + targetDistAbsY + targetDistAbsZ), max(max(targetDistAbsX, targetDistAbsY), targetDistAbsZ));

						const vector3_t stepDistance = bestNode->location - nextNode->location;
						const real_t stepDistAbsX = fabsf(stepDistance.x), stepDistAbsY = fabsf(stepDistance.y), stepDistAbsZ = fabsf(stepDistance.z); 
						const real_t stepCost = heuristicFactor1 * min( heuristicFactor2 * (stepDistAbsX + stepDistAbsY + stepDistAbsZ), max(max(stepDistAbsX, stepDistAbsY), stepDistAbsZ));

						const real_t cost = bestNode->cost + stepCost;
						const real_t total = cost + heuristic;

						if (nextNode->open && nextNode->estimatedTotalCost <= total)
							continue;

						nextNode->cost = cost;
						nextNode->estimatedTotalCost = total;
						nextNode->prevTriangle = bestNodeID;

						if (!nextNode->open)
						{
							nextNode->open = true;
							openList.AddToOpenList(nextTri, nextNode, cost);
						}
					}

					openList.StepDone();
				}

				if (lastBestNodeID.triangleID == inputRequest.To())
				{
					size_t wayTriCount = 0;
					WayTriangleData wayTriangle = lastBestNodeID;
					WayTriangleData nextInsertion(wayTriangle.triangleID, 0);

					while (wayTriangle.triangleID != inputRequest.From())
					{
						const AStarOpenList::Node* node = openList.FindNode(wayTriangle);
						assert(node);
						outputWay[wayTriCount++] = nextInsertion;
						
						//Iterate the path backwards, and shift the offMeshLink to the previous triangle (start of the link)
						nextInsertion.offMeshLinkID = wayTriangle.offMeshLinkID;
						wayTriangle = node->prevTriangle;
						nextInsertion.triangleID = wayTriangle.triangleID;

						if (wayTriCount == maxWayTriCount)
							break;
					}

					if (wayTriCount < maxWayTriCount)
					{
						outputWay[wayTriCount++] = WayTriangleData(inputRequest.From(), nextInsertion.offMeshLinkID);
					}

					return WayQueryResult(wayTriCount, true);
				}
				else if (!openList.Empty())
				{
					//We did not finish yet...
					return WayQueryResult(0, false);
				}
			}
		}

		return WayQueryResult(0, true);
	}

	void MeshGrid::PullString(const vector3_t from, const TriangleID fromTriID, const vector3_t to, const TriangleID toTriID, vector3_t& middlePoint) const
	{
		TileID fromTileID = ComputeTileID(fromTriID);
		TileID toTileID = ComputeTileID(toTriID);

		const TileContainer& startContainer = m_tiles[fromTileID - 1];
		const Tile& startTile = startContainer.tile;
		uint16 fromTriangleIdx = ComputeTriangleIndex(fromTriID);
		const Tile::Triangle& fromTriangle = startTile.triangles[fromTriangleIdx];

		uint16 vi0 = 0,vi1 = 0;
		for(int l=0; l < fromTriangle.linkCount; ++l)
		{
			const Tile::Link& link = startTile.links[fromTriangle.firstLink+l];
			if(link.side == Tile::Link::Internal)
			{
				TriangleID newTriangleID = ComputeTriangleID(fromTileID,link.triangle);
				if(newTriangleID == toTriID)
				{
					vi0 = link.edge;
					vi1 = next_mod3(link.edge);
					assert(vi0 < 3);	 
					assert(vi1 < 3);
					break;
				}
			}
			else
			{
				if(link.side != Tile::Link::OffMesh)
				{
					TileID neighbourTileID = GetNeighbourTileID(startContainer.x, startContainer.y, startContainer.z, link.side);
					TriangleID newTriangleID = ComputeTriangleID(neighbourTileID,link.triangle);

					if(newTriangleID == toTriID)
					{
						vi0 = link.edge;
						vi1 = next_mod3(link.edge);
						assert(vi0 < 3);	 
						assert(vi1 < 3);
						break;
					}
				}
			}
		}

		vector3_t fromVertices[3];
		GetVertices(fromTriID, fromVertices[0], fromVertices[1], fromVertices[2]);

		if(vi0 != vi1)
		{
			real_t s,t;
			vector3_t dir = fromVertices[vi1] - fromVertices[vi0];
			dir.normalized();
			if(IntersectSegmentSegment(vector2_t(fromVertices[vi0]),
				vector2_t(fromVertices[vi1]),vector2_t(from),vector2_t(to),s,t))
			{
				const Tile::Triangle& triangle = startContainer.tile.triangles[ComputeTriangleIndex(fromTriID)];

				s = max(s,kMinPullingThreshold);
				s = min(s,kMaxPullingThreshold);
				middlePoint = fromVertices[vi0] + dir * s;
			}
			else
			{
				if(s < 0)
				{
					middlePoint = fromVertices[vi0] + dir * kMinPullingThreshold;
				}
				else
				{
					middlePoint = fromVertices[vi0] + dir * kMaxPullingThreshold;
				}
			}
		}
	}

	void MeshGrid::AddOffMeshLinkToTile( const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex )
	{
		Tile& tile = GetTile(tileID);

		m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, -(int)tile.linkCount);

		tile.AddOffMeshLink(triangleID, offMeshIndex);		

		m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, tile.linkCount);
	}

	void MeshGrid::UpdateOffMeshLinkForTile(const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex)
	{
		Tile& tile = GetTile(tileID);

		tile.UpdateOffMeshLink(triangleID, offMeshIndex);
	}

	void MeshGrid::RemoveOffMeshLinkFromTile( const TileID tileID, const TriangleID triangleID )
	{
		Tile& tile = GetTile(tileID);

		m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, -(int)tile.linkCount);

		tile.RemoveOffMeshLink(triangleID);

		m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, tile.linkCount);
	}

	inline size_t OppositeSide(size_t side)
	{
		return (side + 7) % 14;
	}

	MeshGrid::ERayCastResult MeshGrid::RayCast(const vector3_t& from, TriangleID fromTri, const vector3_t& to, TriangleID toTri, 
		RaycastRequestBase& raycastRequest) const
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		TileID tileID = ComputeTileID(fromTri);
		const TileContainer* container = &m_tiles[tileID - 1];
		const Tile* tile = &container->tile;

		vector3_t tileOrigin(
			real_t(container->x * m_params.tileSize.x),
			real_t(container->y * m_params.tileSize.y),
			real_t(container->z * m_params.tileSize.z));

		if (fromTri)
		{
			const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(fromTri)];

			const vector2_t a = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[0]]);
			const vector2_t b = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[1]]);
			const vector2_t c = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[2]]);

			if (!PointInTriangle(vector2_t(from), a, b, c))
				fromTri = 0;
		}

		if (!fromTri)
		{
			RayHit& hit = raycastRequest.hit;
			hit.distance = -real_t::max();
			hit.triangleID = 0;
			hit.edge = 0;

			return eRayCastResult_Hit;
		}

		real_t distance = 0;
		size_t triCount = 0;
		TriangleID currentID = fromTri;
		size_t incidentEdge = ~0ul;
		
		while(currentID)
		{
			if (triCount < raycastRequest.maxWayTriCount)
			{
				raycastRequest.way[triCount++] = currentID;
			}
			else
			{
				// We don't allow rays that pass through more than maxWayTriCount triangles
				return eRayCastResult_RayTooLong; 
			}

			if (toTri && currentID == toTri)
			{
				raycastRequest.wayTriCount = triCount;
				return eRayCastResult_NoHit;
			}

			const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(currentID)];
			TriangleID nextID = 0;

			for (size_t e = 0; e < 3; ++e)
			{
				if (incidentEdge == e)
					continue;

				const vector3_t a = tileOrigin + vector3_t(tile->vertices[triangle.vertex[e]]);
				const vector3_t b = tileOrigin + vector3_t(tile->vertices[triangle.vertex[next_mod3(e)]]);

				real_t s, t;
				if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(a), vector2_t(b), s, t))
				{
					if (s < distance || s == 0)
						continue;

					distance = s;

					for (size_t l = 0; l < triangle.linkCount; ++l)
					{
						const Tile::Link& link = tile->links[triangle.firstLink + l];
						if (link.edge != e)
							continue;

						const size_t side = link.side;

						if (side == Tile::Link::Internal)
						{
							const Tile::Triangle& opposite = tile->triangles[link.triangle];

							for (size_t oe = 0; oe < opposite.linkCount; ++oe)
							{
								const Tile::Link& reciprocal = tile->links[opposite.firstLink + oe];
								const TriangleID possibleNextID = ComputeTriangleID(tileID, link.triangle);
								if (reciprocal.triangle == ComputeTriangleIndex(currentID))
								{
									distance = s;
									nextID = possibleNextID;
									incidentEdge = reciprocal.edge;

									break; // opposite edge loop
								}
							}
						}
						else if (side != Tile::Link::OffMesh)
						{
							TileID neighbourTileID = GetNeighbourTileID(container->x, container->y, container->z, link.side);
							const TileContainer& neighbourContainer = m_tiles[neighbourTileID - 1];
							
							const Tile::Triangle& opposite = neighbourContainer.tile.triangles[link.triangle];

							for (size_t rl = 0; rl < opposite.linkCount; ++rl)
							{
								const Tile::Link& reciprocal = neighbourContainer.tile.links[opposite.firstLink + rl];
								if ((reciprocal.triangle == ComputeTriangleIndex(currentID)) && (reciprocal.side == OppositeSide(side)))
								{
									const vector3_t neighbourTileOrigin = vector3_t(
										real_t(neighbourContainer.x * m_params.tileSize.x),
										real_t(neighbourContainer.y * m_params.tileSize.y),
										real_t(neighbourContainer.z * m_params.tileSize.z));

									const uint16 i0 = reciprocal.edge;
									const uint16 i1 = next_mod3(reciprocal.edge);

									assert(i0 < 3);
									assert(i1 < 3);

									const vector3_t c = neighbourTileOrigin + vector3_t(
										neighbourContainer.tile.vertices[opposite.vertex[i0]]);
									const vector3_t d = neighbourTileOrigin + vector3_t(
										neighbourContainer.tile.vertices[opposite.vertex[i1]]);

									const TriangleID possibleNextID = ComputeTriangleID(neighbourTileID, link.triangle);
									real_t p, q;
									if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(c), vector2_t(d), p, q))
									{
										distance = p;
										nextID = possibleNextID;
										incidentEdge = reciprocal.edge;

										tileID = neighbourTileID;
										container = &neighbourContainer;
										tile = &neighbourContainer.tile;

										tileOrigin = neighbourTileOrigin;
									}

									break; // reciprocal link loop
								}
							}
						}

						if (nextID)
							break; // link loop
					}
					if(IsTriangleAlreadyInWay(nextID, raycastRequest.way, triCount))
					{
						assert(0);
						nextID = 0;
					}

					if (nextID)
						break; // edge loop
					else
					{
						RayHit& hit = raycastRequest.hit;
						hit.distance = distance;
						hit.triangleID = currentID;
						hit.edge = e;

						raycastRequest.wayTriCount = triCount;

						return eRayCastResult_Hit;
					}
				}
			}

			currentID = nextID;
		}

		raycastRequest.wayTriCount = triCount;

		bool isEndingTriangleAcceptable = IsTriangleAcceptableForLocation(to, raycastRequest.way[triCount - 1]);
		return isEndingTriangleAcceptable ? eRayCastResult_NoHit : eRayCastResult_Unacceptable;
	}

	bool TestEdgeOverlap1D(const real_t& a0, const real_t& a1,
		const real_t& b0, const real_t& b1, const real_t& dx)
	{
		if ((a0 == b0) && (a1 == b1) && (a0 == b0))
			return true;

		const real_t amin = a0 < a1 ? a0 : a1;
		const real_t amax = a0 < a1 ? a1 : a0;

		const real_t bmin = b0 < b1 ? b0 : b1;
		const real_t bmax = b0 < b1 ? b1 : b0;

		const real_t ominx = std::max(amin + dx, bmin + dx);
		const real_t omaxx = std::min(amax - dx, bmax - dx);

		if (ominx > omaxx)
			return false;

		return true;
	}

	bool TestEdgeOverlap2D(const real_t& toleranceSq, const vector2_t& a0, const vector2_t& a1,
		const vector2_t& b0, const vector2_t& b1, const real_t& dx)
	{
		if ((a0.x == b0.x) && (a1.x == b1.x) && (a0.x == a1.x))
			return TestEdgeOverlap1D(a0.y, a1.y, b0.y, b1.y, dx);

		const vector2_t amin = a0.x < a1.x ? a0 : a1;
		const vector2_t amax = a0.x < a1.x ? a1 : a0;

		const vector2_t bmin = b0.x < b1.x ? b0 : b1;
		const vector2_t bmax = b0.x < b1.x ? b1 : b0;

		const real_t ominx = std::max(amin.x + dx, bmin.x + dx);
		const real_t omaxx = std::min(amax.x - dx, bmax.x - dx);

		if (ominx >= omaxx)
			return false;

		const real_t aslope = ((amax.x - amin.x) != 0) ? ((amax.y - amin.y) / (amax.x - amin.x)) : 0;
		const real_t ab = amin.y - aslope * amin.x;

		const real_t bslope = ((bmax.x - bmin.x) != 0) ? ((bmax.y - bmin.y) / (bmax.x - bmin.x)) : 0;
		const real_t bb = bmin.y - bslope * bmin.x;

		const real_t aominy = ab + aslope * ominx;
		const real_t bominy = bb + bslope * ominx;

		const real_t aomaxy = ab + aslope * omaxx;
		const real_t bomaxy = bb + bslope * omaxx;

		const real_t dminy = bominy - aominy;
		const real_t dmaxy = bomaxy - aomaxy;

		if ((square(dminy) > toleranceSq) || (square(dmaxy) > toleranceSq))
		{
			if ((dminy * dmaxy) >= 0)
				return false;
		}

		return true;
	}

	bool TestEdgeOverlap(size_t side, const real_t& toleranceSq, const vector3_t& a0, const vector3_t& a1,
		const vector3_t& b0, const vector3_t& b1)
	{
		const int ox = NeighbourOffset_MeshGrid[side][0];
		const int oy = NeighbourOffset_MeshGrid[side][1];
		const real_t dx = real_t::fraction(1, 1000);

		if (ox || oy)
		{
			const size_t dim = ox ? 0 : 1;
			const size_t odim = dim ^ 1;

			if ((a1[dim] - a0[dim] != 0) || (b1[dim] - b0[dim] != 0) || (a0[dim] != b0[dim]))
				return false;

			return TestEdgeOverlap2D(toleranceSq, vector2_t(a0[odim], a0.z), vector2_t(a1[odim], a1.z),
				vector2_t(b0[odim], b0.z), vector2_t(b1[odim], b1.z), dx);
		}
		else
		{
			if ((a1.z - a0.z != 0) || (b1.z - b0.z != 0) || (a0.z != b0.z))
				return false;

			return TestEdgeOverlap2D(toleranceSq, vector2_t(a0.x, a0.y), vector2_t(a1.x, a1.y),
				vector2_t(b0.x, b0.y), vector2_t(b1.x, b1.y), dx);
		}
	}

	TileID MeshGrid::SetTile(size_t x, size_t y, size_t z, Tile& tile)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		assert((x <= max_x) || (y <= max_y) || (z <= max_z));

		const size_t tileName = ComputeTileName(x, y, z);

		std::pair<TileMap::iterator, bool> iresult = m_tileMap.insert(TileMap::value_type(tileName, 0));

		size_t tileID;

		if (iresult.second)
		{
			++m_tileCount;

			if (m_frees.empty())
			{
				tileID = m_tileCount;

				if (m_tileCount > m_tileCapacity)
					Grow(std::max<size_t>(4, m_tileCapacity >> 1));
			}
			else
			{
				tileID = m_frees.back() + 1;
				m_frees.pop_back();
			}

			iresult.first->second = tileID;

			m_profiler.AddStat(TileCount, 1);
		}
		else
		{
			tileID = iresult.first->second;

			Tile& oldTile = m_tiles[tileID - 1].tile;
			m_triangleCount -= oldTile.triangleCount;

			m_profiler.AddStat(VertexCount, -(int)oldTile.vertexCount);
			m_profiler.AddStat(TriangleCount, -(int)oldTile.triangleCount);
			m_profiler.AddStat(BVTreeNodeCount, -(int)oldTile.nodeCount);
			m_profiler.AddStat(LinkCount, -(int)oldTile.linkCount);

			m_profiler.FreeMemory(VertexMemory, oldTile.vertexCount * sizeof(Tile::Vertex));
			m_profiler.FreeMemory(TriangleMemory, oldTile.triangleCount * sizeof(Tile::Triangle));
			m_profiler.FreeMemory(BVTreeMemory, oldTile.nodeCount * sizeof(Tile::BVNode));
			m_profiler.FreeMemory(LinkMemory, oldTile.linkCount * sizeof(Tile::Link));

			oldTile.Destroy();
		}

		m_profiler.AddStat(VertexCount, tile.vertexCount);
		m_profiler.AddStat(TriangleCount, tile.triangleCount);
		m_profiler.AddStat(BVTreeNodeCount, tile.nodeCount);
		m_profiler.AddStat(LinkCount, tile.linkCount);

		m_profiler.AddMemory(VertexMemory, tile.vertexCount * sizeof(Tile::Vertex));
		m_profiler.AddMemory(TriangleMemory, tile.triangleCount * sizeof(Tile::Triangle));
		m_profiler.AddMemory(BVTreeMemory, tile.nodeCount * sizeof(Tile::BVNode));
		m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));

		m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used);
		m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type));
		m_profiler.AddMemory(GridMemory, m_tileCapacity * sizeof(TileContainer));
		m_profiler.AddMemory(GridMemory, m_frees.capacity() * sizeof(Frees::value_type));

		m_triangleCount += tile.triangleCount;

		TileContainer& container = m_tiles[tileID - 1];
		container.x = x;
		container.y = y;
		container.z = z;
		container.tile.Swap(tile);
		tile.Destroy();

		return tileID;
	}

	void MeshGrid::ClearTile(TileID tileID, bool clearNetwork)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		assert((tileID > 0) && (tileID <= m_tileCapacity));
		if ((tileID > 0) && (tileID <= m_tileCapacity))
		{
			TileContainer& container = m_tiles[tileID - 1];

			m_profiler.AddStat(VertexCount, -(int)container.tile.vertexCount);
			m_profiler.AddStat(TriangleCount, -(int)container.tile.triangleCount);
			m_profiler.AddStat(BVTreeNodeCount, -(int)container.tile.nodeCount);
			m_profiler.AddStat(LinkCount, -(int)container.tile.linkCount);

			m_profiler.FreeMemory(VertexMemory, container.tile.vertexCount * sizeof(Tile::Vertex));
			m_profiler.FreeMemory(TriangleMemory, container.tile.triangleCount * sizeof(Tile::Triangle));
			m_profiler.FreeMemory(BVTreeMemory, container.tile.nodeCount * sizeof(Tile::BVNode));
			m_profiler.FreeMemory(LinkMemory, container.tile.linkCount * sizeof(Tile::Link));

			m_profiler.AddStat(TileCount, -1);

			m_triangleCount -= container.tile.triangleCount;

			m_frees.push_back(tileID - 1);
			--m_tileCount;

			container.tile.Destroy();

			TileMap::iterator it = m_tileMap.find(ComputeTileName(container.x, container.y, container.z));
			assert(it != m_tileMap.end());
			m_tileMap.erase(it);

			if (clearNetwork)
			{
				const real_t toleranceSq = square(real_t(std::max(m_params.voxelSize.x, m_params.voxelSize.z)));

				for (size_t side = 0; side < SideCount; ++side)
				{
					size_t nx = container.x + NeighbourOffset_MeshGrid[side][0];
					size_t ny = container.y + NeighbourOffset_MeshGrid[side][1];
					size_t nz = container.z + NeighbourOffset_MeshGrid[side][2];

					if (TileID neighbourID = GetTileID(nx, ny, nz))
					{
						TileContainer& ncontainer = m_tiles[neighbourID - 1];

						ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, toleranceSq, ncontainer.tile,
							OppositeSide(side), container.x, container.y, container.z, tileID);
					}
				}
			}
	
			m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used);
			m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type));
			m_profiler.AddMemory(GridMemory, m_tileCapacity * sizeof(TileContainer));
			m_profiler.AddMemory(GridMemory, m_frees.capacity() * sizeof(Frees::value_type));
		}
	}

	void MeshGrid::CreateNetwork()
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		TileMap::iterator it = m_tileMap.begin();
		TileMap::iterator end = m_tileMap.end();

		const real_t toleranceSq = square(real_t(std::max(m_params.voxelSize.x, m_params.voxelSize.z)));

		for ( ; it != end; ++it)
		{
			const TileID tileID = it->second;
			
			TileContainer& container = m_tiles[tileID - 1];

			ComputeAdjacency(container.x, container.y, container.z, toleranceSq, container.tile);
		}
	}

	void MeshGrid::ConnectToNetwork(TileID tileID)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		const real_t toleranceSq = square(real_t(std::max(m_params.voxelSize.x, m_params.voxelSize.z)));

		assert((tileID > 0) && (tileID <= m_tileCapacity));
		if ((tileID > 0) && (tileID <= m_tileCapacity))
		{
			TileContainer& container = m_tiles[tileID - 1];

			ComputeAdjacency(container.x, container.y, container.z, toleranceSq, container.tile);

			for (size_t side = 0; side < SideCount; ++side)
			{
				size_t nx = container.x + NeighbourOffset_MeshGrid[side][0];
				size_t ny = container.y + NeighbourOffset_MeshGrid[side][1];
				size_t nz = container.z + NeighbourOffset_MeshGrid[side][2];

				if (TileID neighbourID = GetTileID(nx, ny, nz))
				{
					TileContainer& ncontainer = m_tiles[neighbourID - 1];

					//ComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, toleranceSq, ncontainer.tile);
					ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, toleranceSq, ncontainer.tile,
						OppositeSide(side), container.x, container.y, container.z, tileID);
				}
			}
		}
	}

	TileID MeshGrid::GetTileID(size_t x, size_t y, size_t z) const
	{
		const size_t tileName = ComputeTileName(x, y, z);

		TileMap::const_iterator it = m_tileMap.find(tileName);
		if (it != m_tileMap.end())
			return it->second;
		return 0;
	}

	const Tile& MeshGrid::GetTile(TileID tileID) const
	{
		assert(tileID > 0);
		assert(tileID <= m_tileCapacity);
		return m_tiles[tileID - 1].tile;
	}

	Tile& MeshGrid::GetTile(TileID tileID)
	{
		assert(tileID > 0);
		assert(tileID <= m_tileCapacity);
		return m_tiles[tileID - 1].tile;
	}

	void MeshGrid::Swap(MeshGrid& other)
	{
		std::swap(m_tiles, other.m_tiles);
		std::swap(m_tileCount, other.m_tileCount);
		std::swap(m_tileCapacity, other.m_tileCapacity);
		std::swap(m_triangleCount, other.m_triangleCount);

		m_frees.swap(other.m_frees);
		m_tileMap.swap(other.m_tileMap);

		std::swap(m_params, other.m_params);
		std::swap(m_profiler, other.m_profiler);
	}

	void MeshGrid::Draw(size_t drawFlags, TileID excludeID) const
	{
		TileMap::const_iterator it = m_tileMap.begin();
		TileMap::const_iterator end = m_tileMap.end();

		for ( ; it != end; ++it)
		{
			if (excludeID == it->second)
				continue;

			const TileContainer& container = m_tiles[it->second - 1];

			container.tile.Draw(drawFlags, vector3_t(
				real_t(m_params.origin.x + container.x * m_params.tileSize.x),
				real_t(m_params.origin.y + container.y * m_params.tileSize.y),
				real_t(m_params.origin.z + container.z * m_params.tileSize.z)));

			const Vec3 offset = Vec3(0.0f, 0.0f, 0.05f) + m_params.origin +
				Vec3((float)container.x * m_params.tileSize.x,
				(float)container.y * m_params.tileSize.y,
				(float)container.z * m_params.tileSize.z);
		}
	}

	const MeshGrid::ProfilerType& MeshGrid::GetProfiler() const
	{
		return m_profiler;
	}

	struct Edge
	{
		uint16 vertex[2];
		uint16 triangle[2];
	};

	void ComputeTileTriangleAdjacency(Tile::Triangle* triangles, size_t triangleCount, size_t vertexCount,
		Edge* edges, uint16* adjacency)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		{
			enum { Unused = 0xffff, };

			const size_t MaxLookUp = 4096;
			uint16 edgeLookUp[MaxLookUp];
			assert(MaxLookUp > vertexCount + triangleCount * 3);

			std::fill(&edgeLookUp[0], &edgeLookUp[0] + vertexCount, static_cast<uint16>(Unused));

			size_t edgeCount = 0;

			for (size_t i = 0; i < triangleCount; ++i)
			{
				const Tile::Triangle& triangle = triangles[i];

				for (size_t v = 0; v < 3; ++v)
				{
					uint16 i1 = triangle.vertex[v];
					uint16 i2 = triangle.vertex[next_mod3(v)];

					if (i1 < i2)
					{
						size_t edgeIdx = edgeCount++;
						adjacency[i * 3 + v] = edgeIdx;

						Edge& edge = edges[edgeIdx];
						edge.triangle[0] = i;
						edge.triangle[1] = i;
						edge.vertex[0] = i1;
						edge.vertex[1] = i2;

						edgeLookUp[vertexCount + edgeIdx] = edgeLookUp[i1];
						edgeLookUp[i1] = edgeIdx;
					}
				}
			}

			for (size_t i = 0; i < triangleCount; ++i)
			{
				const Tile::Triangle& triangle = triangles[i];

				for (size_t v = 0; v < 3; ++v)
				{
					uint16 i1 = triangle.vertex[v];
					uint16 i2 = triangle.vertex[next_mod3(v)];

					if (i1 > i2)
					{
						size_t edgeIndex = edgeLookUp[i2];

						for ( ; edgeIndex != Unused; edgeIndex = edgeLookUp[vertexCount + edgeIndex])
						{
							Edge& edge = edges[edgeIndex];

							if ((edge.vertex[1] == i1) && (edge.triangle[0] == edge.triangle[1]))
							{
								edge.triangle[1] = i;
								adjacency[i * 3 + v] = edgeIndex;
								break;
							}
						}

						if (edgeIndex == Unused)
						{
							size_t edgeIdx = edgeCount++;
							adjacency[i * 3 + v] = edgeIdx;

							Edge& edge = edges[edgeIdx];
							edge.vertex[0] = i1;
							edge.vertex[1] = i2;
							edge.triangle[0] = i;
							edge.triangle[1] = i;
						}
					}
				}
			}
		}
	}

	void MeshGrid::Grow(size_t amount)
	{
		const size_t oldCapacity = m_tileCapacity;
		
		m_tileCapacity = m_tileCapacity + amount;
		TileContainer* tiles = new TileContainer[m_tileCapacity];

		if (oldCapacity)
			memcpy(tiles, m_tiles, oldCapacity * sizeof(TileContainer));

		std::swap(m_tiles, tiles);
		delete[] tiles;
	}

	TileID MeshGrid::GetNeighbourTileID(size_t x, size_t y, size_t z, size_t side) const
	{
		size_t nx = x + NeighbourOffset_MeshGrid[side][0];
		size_t ny = y + NeighbourOffset_MeshGrid[side][1];
		size_t nz = z + NeighbourOffset_MeshGrid[side][2];

		return GetTileID(nx, ny, nz);
	}

	static const size_t MaxTriangleCount = 1024;

	struct SideTileInfo
	{
		SideTileInfo()
			: tile(0)
		{
		}

		Tile* tile;
		vector3_t offset;
	};

#pragma warning (push)
#pragma warning (disable: 6262)
	void MeshGrid::ComputeAdjacency(size_t x, size_t y, size_t z, const real_t& toleranceSq, Tile& tile)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		const size_t vertexCount = tile.vertexCount;
		const Tile::Vertex* vertices = tile.vertices;

		const size_t triCount = tile.triangleCount;
		if (!triCount)
			return;

		m_profiler.StartTimer(NetworkConstruction);

		assert(triCount <= MaxTriangleCount);

		Edge edges[MaxTriangleCount * 3];
		uint16 adjacency[MaxTriangleCount * 3];
		ComputeTileTriangleAdjacency(tile.triangles, triCount, vertexCount, edges, adjacency);

		const size_t MaxLinkCount = MaxTriangleCount * 6;
		Tile::Link links[MaxLinkCount];
		size_t linkCount = 0;

		SideTileInfo sides[SideCount];

		for (size_t s = 0; s < SideCount; ++s)
		{
			SideTileInfo& side = sides[s];

			if (TileID id = GetTileID(x + NeighbourOffset_MeshGrid[s][0], y + NeighbourOffset_MeshGrid[s][1], z + NeighbourOffset_MeshGrid[s][2]))
			{
				side.tile = &GetTile(id);
				side.offset = vector3_t(
					NeighbourOffset_MeshGrid[s][0] * m_params.tileSize.x,
					NeighbourOffset_MeshGrid[s][1] * m_params.tileSize.y,
					NeighbourOffset_MeshGrid[s][2] * m_params.tileSize.z);
			}
		}

		for (size_t i = 0; i < triCount; ++i)
		{
			size_t triLinkCount = 0;

			for (size_t e = 0; e < 3; ++e)
			{
				const size_t edgeIndex = adjacency[i * 3 + e];
				const Edge& edge = edges[edgeIndex];
				if ((edge.triangle[0] != i) && (edge.triangle[1] != i))
					continue;

				if (edge.triangle[0] != edge.triangle[1])
				{
					Tile::Link& link = links[linkCount++];
					link.side = Tile::Link::Internal;
					link.edge = e;
					link.triangle = (edge.triangle[1] == i) ? edge.triangle[0] : edge.triangle[1];

					++triLinkCount;
				}
			}

			if (triLinkCount == 3)
			{
				Tile::Triangle& triangle = tile.triangles[i];
				triangle.linkCount = triLinkCount;
				triangle.firstLink = linkCount - triLinkCount;

				continue;
			}

			for (size_t e = 0; e < 3; ++e)
			{
				const size_t edgeIndex = adjacency[i * 3 + e];
				const Edge& edge = edges[edgeIndex];
				if ((edge.triangle[0] != i) && (edge.triangle[1] != i))
					continue;

				if (edge.triangle[0] != edge.triangle[1])
					continue;

				const vector3_t a0 = vector3_t(vertices[edge.vertex[0]]);
				const vector3_t a1 = vector3_t(vertices[edge.vertex[1]]);

				for (size_t s = 0; s < SideCount; ++s)
				{
					if (const Tile* stile = sides[s].tile)
					{
						const vector3_t& offset = sides[s].offset;

						const size_t striangleCount = stile->triangleCount;
						const Tile::Triangle* striangles = stile->triangles;
						const Tile::Vertex* svertices = stile->vertices;

						for (size_t k = 0; k < striangleCount; ++k)
						{
							const Tile::Triangle& striangle = striangles[k];

							for (size_t ne = 0; ne < 3; ++ne)
							{
								const vector3_t b0 = offset + vector3_t(svertices[striangle.vertex[ne]]);
								const vector3_t b1 = offset + vector3_t(svertices[striangle.vertex[next_mod3(ne)]]);
								
								if (TestEdgeOverlap(s, toleranceSq, a0, a1, b0, b1))
								{
									Tile::Link& link = links[linkCount++];
									link.side = s;
									link.edge = e;
									link.triangle = k;

#if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
									const TileID checkId = GetTileID(x + NeighbourOffset_MeshGrid[s][0], y + NeighbourOffset_MeshGrid[s][1], z + NeighbourOffset_MeshGrid[s][2]);
									BreakOnInvalidTriangle( ComputeTriangleID( checkId, k ), m_tileCount );
#endif

									++triLinkCount;
									break;
								}
							}
						}
					}
				}
			}

			Tile::Triangle& triangle = tile.triangles[i];
			triangle.linkCount = triLinkCount;
			triangle.firstLink = linkCount - triLinkCount;
		}

		m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, -(int)tile.linkCount);

		tile.CopyLinks(links, linkCount);

		m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
		m_profiler.AddStat(LinkCount, tile.linkCount);
		m_profiler.StopTimer(NetworkConstruction);
	}
#pragma warning (pop)

	void MeshGrid::ReComputeAdjacency(size_t x, size_t y, size_t z, const real_t& toleranceSq, Tile& tile,
		size_t side, size_t tx, size_t ty, size_t tz, TileID targetID)
	{
		FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);

		const size_t vertexCount = tile.vertexCount;
		const Tile::Vertex* vertices = tile.vertices;

		const size_t triangleCount = tile.triangleCount;
		if (!triangleCount)
			return;

		m_profiler.StartTimer(NetworkConstruction);

		if (!tile.linkCount)
			ComputeAdjacency(x, y, z, toleranceSq, tile);
		else
		{
			const vector3_t noffset = vector3_t(
				NeighbourOffset_MeshGrid[side][0] * m_params.tileSize.x,
				NeighbourOffset_MeshGrid[side][1] * m_params.tileSize.y,
				NeighbourOffset_MeshGrid[side][2] * m_params.tileSize.z);

			assert(triangleCount <= MaxTriangleCount);

			const size_t MaxLinkCount = MaxTriangleCount * 6;
			Tile::Link links[MaxLinkCount];
			size_t linkCount = 0;

			const Tile::AABB& aabb = tile.nodes[0].aabb;
			const Tile::Link* olinks = tile.links;

			const Tile* target = targetID ? &m_tiles[targetID - 1].tile : 0;

			const Tile::Vertex* tvertices = targetID ? target->vertices : 0;
			
			const size_t ttriangleCount = target ? target->triangleCount : 0;
			const Tile::Triangle* ttriangles = target ? target->triangles : 0;

			for (size_t i = 0; i < triangleCount; ++i)
			{
				size_t triLinkCount = 0;
				size_t triLinkCountI = 0;

				Tile::Triangle& triangle = tile.triangles[i];
				const size_t olinkCount = triangle.linkCount;
				const size_t ofirstLink = triangle.firstLink;

				for (size_t l = 0; l < olinkCount; ++l)
				{
					const Tile::Link& olink = tile.links[ofirstLink + l];

					if ((olink.side == Tile::Link::Internal) || (olink.side != side))
					{
						links[linkCount++] = olink;
						++triLinkCount;
						triLinkCountI += (olink.side == Tile::Link::Internal) ? 1 : 0;
					}
				}

				if ((triLinkCountI == 3) || !targetID)
				{
					triangle.linkCount = triLinkCount;
					triangle.firstLink = linkCount - triLinkCount;

					continue;
				}

				for (size_t e = 0; e < 3; ++e)
				{
					const vector3_t a0 = vector3_t(vertices[triangle.vertex[e]]);
					const vector3_t a1 = vector3_t(vertices[triangle.vertex[next_mod3(e)]]);

					for (size_t k = 0; k < ttriangleCount; ++k)
					{
						const Tile::Triangle& ttriangle = ttriangles[k];

						for (size_t ne = 0; ne < 3; ++ne)
						{
							const vector3_t b0 = noffset + vector3_t(tvertices[ttriangle.vertex[ne]]);
							const vector3_t b1 = noffset + vector3_t(tvertices[ttriangle.vertex[next_mod3(ne)]]);

							if (TestEdgeOverlap(side, toleranceSq, a0, a1, b0, b1))
							{
								Tile::Link& link = links[linkCount++];
								link.side = side;
								link.edge = e;
								link.triangle = k;

#if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
								const TileID checkId = GetTileID(x + NeighbourOffset_MeshGrid[side][0], y + NeighbourOffset_MeshGrid[side][1], z + NeighbourOffset_MeshGrid[side][2]);
								BreakOnInvalidTriangle( ComputeTriangleID( checkId, k ), m_tileCount );
#endif
								++triLinkCount;
								break;
							}
						}
					}
				}

				triangle.linkCount = triLinkCount;
				triangle.firstLink = linkCount - triLinkCount;	
			}

			m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
			m_profiler.AddStat(LinkCount, -(int)tile.linkCount);

			tile.CopyLinks(links, linkCount);

			m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link));
			m_profiler.AddStat(LinkCount, tile.linkCount);
		}

		m_profiler.StopTimer(NetworkConstruction);
	}

	bool MeshGrid::CalculateMidEdge(const TriangleID triangleID1, const TriangleID triangleID2, Vec3& result) const
	{
		if (triangleID1 == triangleID2)
			return false;

		const TileID tileID = ComputeTileID(triangleID1);
		const TileContainer& container = m_tiles[tileID - 1];
		const Tile& tile = container.tile;
		const uint16 triangleIdx = ComputeTriangleIndex(triangleID1);
		const Tile::Triangle& triangle = tile.triangles[triangleIdx];

		uint16 vi0 = 0, vi1 = 0;

		for (size_t l = 0; l < triangle.linkCount; ++l)
		{
			const Tile::Link& link = tile.links[triangle.firstLink + l];

			if (link.side == Tile::Link::Internal)
			{
				TriangleID linkedTriID = ComputeTriangleID(tileID, link.triangle);
				if (linkedTriID == triangleID2)
				{
					vi0 = link.edge;
					vi1 = (link.edge + 1) % 3;
					assert(vi0 < 3);
					assert(vi1 < 3);
					break;
				}
			}
			else if (link.side != Tile::Link::OffMesh)
			{
				TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side);
				TriangleID linkedTriID = ComputeTriangleID(neighbourTileID, link.triangle);
				if (linkedTriID == triangleID2)
				{
					vi0 = link.edge;
					vi1 = (link.edge + 1) % 3;
					assert(vi0 < 3);
					assert(vi1 < 3);
					break;
				}
			}
		}

		if (vi0 != vi1)
		{
			vector3_t v0, v1, v2;
			GetVertices(triangleID1, v0, v1, v2);
			vector3_t vertices[3] = {v0, v1, v2};

			result = (vertices[vi0] + vertices[vi1]).GetVec3() * 0.5f;
			return true;
		}

		return false;
	}

	//////////////////////////////////////////////////////////////////////////

#if MNM_USE_EXPORT_INFORMATION

	void MeshGrid::ResetAccessibility(uint8 accessible)
	{
		for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt)
		{
			Tile& tile = m_tiles[tileIt->second - 1].tile;

			tile.ResetConnectivity(accessible);
		}
	}

	void MeshGrid::ComputeAccessibility( const AccessibilityRequest& inputRequest )
	{
		struct Node 
		{
			Node (TriangleID _id, TriangleID _previousID)
				: id(_id)
				, previousId(_previousID)
			{

			}

			TriangleID id;
			TriangleID previousId;
		};

		std::vector<TriangleID> nextTriangles;
		nextTriangles.reserve(16);

		std::vector<Node>		openNodes;
		openNodes.reserve(m_triangleCount);

		const TileID startTileID = ComputeTileID(inputRequest.From());
		const uint16 startTriangleIdx = ComputeTriangleIndex(inputRequest.From());
		Tile& startTile = m_tiles[startTileID - 1].tile;
		startTile.SetTriangleAccessible(startTriangleIdx);

		openNodes.push_back(Node(inputRequest.From(), 0));

		while (!openNodes.empty())
		{
			const Node currentNode = openNodes.back();
			openNodes.pop_back();

			const TileID tileID = ComputeTileID(currentNode.id);
			const uint16 triangleIdx = ComputeTriangleIndex(currentNode.id);
			
			TileContainer& container = m_tiles[tileID - 1];
			Tile& tile = container.tile;

			const Tile::Triangle& triangle = tile.triangles[triangleIdx];

			nextTriangles.clear();

			// Collect all accessible nodes from the current one
			for (size_t l = 0; l < triangle.linkCount; ++l)
			{
				const Tile::Link& link = tile.links[triangle.firstLink + l];

				TriangleID nextTriangleID;

				if (link.side == Tile::Link::Internal)
				{
					nextTriangleID = ComputeTriangleID(tileID, link.triangle);
				}
				else if (link.side != Tile::Link::OffMesh)
				{
					TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side);
					nextTriangleID = ComputeTriangleID(neighbourTileID, link.triangle);
				}
				else
				{
					OffMeshNavigation::QueryLinksResult links = inputRequest.GetOffMeshNavigation().GetLinksForTriangle(currentNode.id, link.triangle);
					while(WayTriangleData nextLink = links.GetNextTriangle())
					{
						nextTriangles.push_back(nextLink.triangleID);
					}
					continue;
				}

				nextTriangles.push_back(nextTriangleID);
			}

			// Add them to the open list if not visited already
			for (size_t t = 0; t < nextTriangles.size(); ++t)
			{
				const TriangleID nextTriangleID = nextTriangles[t];

				// Skip if previous triangle
				if (nextTriangleID == currentNode.previousId)
					continue;

				// Skip if already visited
				const TileID nextTileID = ComputeTileID(nextTriangleID);
				Tile& nextTile = m_tiles[nextTileID - 1].tile;
				const uint16 nextTriangleIndex = ComputeTriangleIndex(nextTriangleID);
				if (nextTile.IsTriangleAccessible(nextTriangleIndex))
					continue;

				nextTile.SetTriangleAccessible(nextTriangleIndex);

				openNodes.push_back(Node(nextTriangleID, currentNode.id));
			}
		}
	}

#endif
}
